#include <cmath>
#include <cstdlib>
#include <vector>

#include <PA9.h>

#include "Blob.h"
#include "Constants.h"

#define PI 3.141

namespace BlobSalladDS
{
	Blob::Blob(double x, double y, double radius, int numPointMasses)
	{
		this->x = x;
		this->y = y;
		this->sticks = new std::vector<Stick *>(numPointMasses);
		this->pointMasses = new std::vector<PointMass *>(numPointMasses);
		this->joints = new std::vector<Joint *>(numPointMasses);
		this->middlePointMass = new PointMass(x, y, 1.0);
		this->radius = radius;
		this->drawFaceStyle = 1;
		this->drawEyeStyle = 1;
		this->selected = false;

		double low = 0.95, high = 1.05;
		int i, p;
		double t = 0;

		for (i = 0; i < numPointMasses; i++)
		{
			this->pointMasses->at(i) = new PointMass(cos(t) * radius + x, sin(t) * radius + y, 1.0);
			t += 2.0 * PI / numPointMasses;

/*
			PA_OutputText(
				SCREEN_TOP,
				2,
				2 * i,
				"t:%f10 y:%f10",
				this->pointMasses->at(i)->getXPos(),
				this->pointMasses->at(i)->getYPos()
			);
*/
		}

		this->pointMasses->at(0)->setMass(2.0);
		this->pointMasses->at(1)->setMass(2.0);

		for (i = 0; i < numPointMasses; i++)
		{
			this->sticks->at(i) = new Stick(this->pointMasses->at(i), this->pointMasses->at(clampIndex(i + 1, numPointMasses)));
		}

		for (i = 0, p = 0; i < numPointMasses; i++, p++)
		{
			this->joints->at(p) = new Joint(this->pointMasses->at(i), this->pointMasses->at(clampIndex(i + numPointMasses / 2 + 1, numPointMasses)), low, high);
			this->joints->at(p) = new Joint(this->pointMasses->at(i), this->middlePointMass, high * 0.9, low * 1.1); // 0.8, 1.2 works
		}
	}

	double Blob::drand()
	{
		return rand() / RAND_MAX * 1.0;
	}

	int Blob::clampIndex(int index, int maxIndex)
	{
		index += maxIndex;
		return index % maxIndex;
	}

	void Blob::addBlob(Blob &blob)
	{
		size_t index = this->joints->size();
		double dist;

		this->joints->at(index) = new Joint(this->middlePointMass, blob.getMiddlePointMass(), 0.0, 0.0);
		dist = this->radius + blob.getRadius();
		this->joints->at(index)->setDist(dist * 0.95, 0.0);
	}

	PointMass *Blob::getMiddlePointMass()
	{
		return this->middlePointMass;
	}

	double Blob::getRadius()
	{
		return this->radius;
	}

	double Blob::getXPos()
	{
		return this->middlePointMass->getXPos();
	}

	double Blob::getYPos()
	{
		return this->middlePointMass->getYPos();
	}

	void Blob::scale(double scaleFactor)
	{
		size_t i;

		for (i = 0; i < this->joints->size(); i++)
		{
			this->joints->at(i)->scale(scaleFactor);
		}

		for (i = 0; i < this->sticks->size(); i++)
		{
			this->sticks->at(i)->scale(scaleFactor);
		}

		this->radius *= scaleFactor;
	}

	void Blob::move(double dt)
	{
		size_t i;

		for (i = 0; i < this->pointMasses->size(); i++)
		{
			this->pointMasses->at(i)->move(dt);
		}

		this->middlePointMass->move(dt);
	}

	void Blob::sc(Environment &env)
	{
		size_t i, j;

		for (j = 0; j < 4; j++)
		{
			for (i = 0; i < this->pointMasses->size(); i++)
			{
				if (env.collision(*(this->pointMasses->at(i)->getPos()), *(this->pointMasses->at(i)->getPrevPos())))
				{
					this->pointMasses->at(i)->setFriction(0.75);
				}
				else
				{
					this->pointMasses->at(i)->setFriction(0.01);
				}
			}

			for (i = 0; i < this->sticks->size(); i++)
			{
				this->sticks->at(i)->sc(env);
			}

			for (i = 0; i < this->joints->size(); i++)
			{
				this->joints->at(i)->sc();
			}
		}
	}

	void Blob::setForce(Vector *force)
	{
		size_t i;

		for (i = 0; i < this->pointMasses->size(); i++)
		{
			this->pointMasses->at(i)->setForce(*force);
		}

		this->middlePointMass->setForce(*force);
	}

	void Blob::addForce(Vector *force)
	{
		size_t i;

		for (i = 0; i < this->pointMasses->size(); i++)
		{
			this->pointMasses->at(i)->addForce(*force);
		}

		this->middlePointMass->addForce(*force);
	}

	void Blob::moveTo(double x, double y)
	{
		size_t i;
		Vector *blobPos;

		blobPos = this->middlePointMass->getPos();
		x -= blobPos->getX();
		y -= blobPos->getY();

		for (i = 0; i < this->pointMasses->size(); i++)
		{
			blobPos = this->pointMasses->at(i)->getPos();
			blobPos->addX(x);
			blobPos->addY(y);
		}

		blobPos = this->middlePointMass->getPos();
		blobPos->addX(x);
		blobPos->addY(y);
	}

	void Blob::setSelected(bool selected)
	{
		this->selected = selected;
	}

	void Blob::drawEars(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawHappyEyes1(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawHappyEyes2(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawHappyFace1(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawHappyFace2(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawOohFace(void * ctx, double scaleFactor)
	{
	}

	void Blob::drawFace(void * ctx, double scaleFactor)
	{
		srand(time(NULL));

		if (this->drawFaceStyle == 1 && drand() < 0.05)
		{
			this->drawFaceStyle = 2;
		}
		else if (this->drawFaceStyle == 2 && drand() < 0.1)
		{
			this->drawFaceStyle = 1;
		}

		if (this->drawEyeStyle == 1 && drand() < 0.025)
		{
			this->drawEyeStyle = 2;
		}
		else if (this->drawEyeStyle == 2 && drand() < 0.3)
		{
			this->drawEyeStyle = 1;
		}

		if (this->middlePointMass->getVelocity() > 0.004)
		{
			this->drawOohFace(ctx, scaleFactor);
		}
		else
		{
			if (this->drawFaceStyle == 1)
			{
				this->drawHappyFace1(ctx, scaleFactor);
			}
			else
			{
				this->drawHappyFace2(ctx, scaleFactor);
			}

			if (this->drawEyeStyle == 1)
			{
				this->drawHappyEyes1(ctx, scaleFactor);
			}
			else
			{
				this->drawHappyEyes2(ctx, scaleFactor);
			}
		}
	}

	PointMass *Blob::getPointMass(int index)
	{
		index += this->pointMasses->size();
		index = index % this->pointMasses->size();
		return this->pointMasses->at(index);
	}

	void Blob::drawBody(void * ctx, double scaleFactor)
	{
		u16 strokeStyle = PA_RGB(0, 0, 0);
		u16 fillStyle;

		if (this->selected)
		{
			fillStyle = PA_RGB(31, 28, 28);
		}
		else
		{
			fillStyle = PA_RGB(31, 31, 31);
		}

		//int lineWidth = 3;

		//this->moveTo(
		//	this->pointMasses->at(0)->getXPos() * scaleFactor,
		//	this->pointMasses->at(0)->getYPos() * scaleFactor
		//);
		/*
		PA_OutputText(
				SCREEN_TOP,
				2,
				2,
				"x:%d y:%d",
				this->pointMasses->at(0)->getXPos(),
				this->pointMasses->at(0)->getYPos()
			);
			*/

		for (size_t i = 0; i < this->pointMasses->size(); i++)
		{
			double px, py, nx, ny, tx, ty, cx, cy;
			PointMass *prevPointMass, *currentPointMass, *nextPointMass, *nextNextPointMass;

			prevPointMass = this->getPointMass(i - 1);
			currentPointMass = this->pointMasses->at(i);
			nextPointMass = this->getPointMass(i + 1);
			nextNextPointMass = this->getPointMass(i + 2);

			tx = nextPointMass->getXPos();
			ty = nextPointMass->getYPos();

			cx = currentPointMass->getXPos();
			cy = currentPointMass->getYPos();
/*
			PA_OutputText(
				SCREEN_TOP,
				2,
				2,
				"px:%f3 py:%f3 tx:%f3 ty:%f3",
				cx, cy, tx, ty
			);
*/
			px = cx * 0.5 + tx * 0.5;
			py = cy * 0.5 + ty * 0.5;

			nx = cx - prevPointMass->getXPos() + tx - nextNextPointMass->getXPos();
			ny = cy - prevPointMass->getYPos() + ty - nextNextPointMass->getYPos();

			px += nx * 0.16;
			py += ny * 0.16;

			px = px * scaleFactor;
			py = py * scaleFactor;

			tx = tx * scaleFactor;
			ty = ty * scaleFactor;

			//ctx.bezierCurveTo(px, py, tx, ty, tx, ty);

			//TODO: Draw curve
			PA_Draw16bitLine(SCREEN_BOTTOM, (int)px, (int)py, (int)tx, (int)ty, strokeStyle);

			PA_OutputText(
				SCREEN_TOP,
				0,
				i,
				"px:%f3 py:%f3 tx:%f3 ty:%f3",
				px, py, tx, ty
			);
		}
	}

	void Blob::drawSimpleBody(void * ctx, double scaleFactor)
	{
		size_t i;
		for (i = 0; i < this->sticks->size(); i++)
		{
			this->sticks->at(i)->draw(ctx, scaleFactor);
		}
	}

	void Blob::draw(void * ctx, double scaleFactor)
	{
		drawBody(ctx, scaleFactor);
	}
}
